def on_button_pressed_a():
    global mode
    mode = "S"
    basic.show_string("S")
input.on_button_pressed(Button.A, on_button_pressed_a)

def on_button_pressed_b():
    global mode
    mode = "N"
    basic.show_string("N")
input.on_button_pressed(Button.B, on_button_pressed_b)

def on_logo_pressed():
    global mode
    mode = "E"
    basic.show_string("E")
input.on_logo_event(TouchButtonEvent.PRESSED, on_logo_pressed)

pre_pedal = 0
diff = 0
pedal = 0
mode = ""
led.set_brightness(41)
diff_sum = 0
# Exciting mode (N = normal, S = Sports)
mode = "E"
basic.show_string("E")
pins.analog_write_pin(AnalogPin.P2, 1023)
OLED.init(128, 64)
OLED.write_string_new_line("V6 engine car")
OLED.write_string_new_line("pedal differential demo")
basic.pause(2000)
speed = 320
pins.analog_write_pin(AnalogPin.P2, 320)
OLED.clear()
basic.pause(2000)

def on_forever():
    global pedal, diff, diff_sum, speed, pre_pedal
    # read pedal
    pedal = pins.analog_read_pin(AnalogPin.P1)
    # calculate diff
    diff = pedal - pre_pedal
    if mode == "N":
        # dont' care diff, clear it
        diff = 0
    elif mode == "S":
        if diff < 0:
            # only cares positive diff, don't care negative diff, clear it
            diff = 0
    else:
        # 'Exciting' mode. cars positive and negative diff.
        # no action required here actually
        # 
        control.wait_micros(4)
    diff_sum += diff
    # unbalancing.
    # wider range for positive diff sum than negative
    diff_sum = Math.constrain(diff_sum, -150, 500)
    # decide speed output (pwm value) by (pedal+diff_sum),
    # for safe, add limit check
    speed = Math.constrain(pins.map(pedal + diff_sum, 350, 1023, 330, 1023), 330, 1023)
    # output to motor driver chip ULN2803
    pins.analog_write_pin(AnalogPin.P2, speed)
    # update previous pedal value
    pre_pedal = pedal
    # do this 100 times every 1 second
    basic.pause(10)
basic.forever(on_forever)

# OLED dispaly
# running parameters

def on_in_background():
    while True:
        OLED.clear()
        basic.pause(10)
        OLED.write_num_new_line(pedal)
        OLED.write_string_new_line("")
        basic.pause(10)
        OLED.write_num_new_line(diff_sum)
        OLED.write_string_new_line("")
        basic.pause(10)
        OLED.write_num_new_line(speed)
        basic.pause(600)
control.in_background(on_in_background)

def on_every_interval():
    global diff_sum
    if diff_sum > 0:
        # dec to 0 if sum > 0
        diff_sum += -1
    elif diff_sum < 0:
        # inc to 0 if sum < 0
        diff_sum += 1
loops.every_interval(2, on_every_interval)
